#include <WiFi.h>
#include <WiFiUdp.h>

// ===================== 配置区 =====================
// WiFi 热点配置
static const char* AP_SSID = "esp32_motor_ap";
static const char* AP_PASSWORD = "12345678";
static const uint16_t UDP_PORT = 4210;

// 引脚定义
static const int PIN_PWM1 = 18; // PWM1（正转控制）
static const int PIN_PWM2 = 19; // PWM2（反转控制）

// PWM 配置：ESP32 LEDC 定时器
static const int LEDC_CHANNEL_PWM1 = 0; // LEDC 通道 0
static const int LEDC_CHANNEL_PWM2 = 1; // LEDC 通道 1
static const int LEDC_TIMER_BITS = 8;   // 8位分辨率（0-255）
static const int LEDC_FREQUENCY = 20000; // 20kHz

// 安全配置
static const unsigned long CONNECTION_TIMEOUT = 1000; // 连接超时时间（毫秒）
static const unsigned long SAFETY_CHECK_INTERVAL = 100; // 安全检查间隔（毫秒）

// ===================== 全局变量 =====================
WiFiUDP Udp;
bool dualmos_inited = false;
unsigned long lastPacketTime = 0; // 最后收到数据包的时间
unsigned long lastSafetyCheck = 0; // 最后安全检查时间
bool emergencyStop = false; // 紧急停止标志

// ===================== 工具函数 =====================

// 将 0-255 的速度值写入到指定的 LEDC 通道
static inline void writeSpeedToChannel(int ledcChannel, int speed) {
  // 速度限制在 0-255
  if (speed < 0) speed = 0;
  if (speed > 255) speed = 255;
  ledcWrite(ledcChannel, speed);
}

// 停止所有电机
void stopAllMotors() {
  Serial.println("停止所有电机...");
  writeSpeedToChannel(LEDC_CHANNEL_PWM1, 0);
  writeSpeedToChannel(LEDC_CHANNEL_PWM2, 0);
}

// 紧急停止函数
void emergencyStopMotors() {
  if (!emergencyStop) {
    Serial.println("执行紧急停止！");
    stopAllMotors();
    emergencyStop = true;
  }
}

// 检查连接状态，如果超时则停止电机
void checkConnection() {
  unsigned long currentTime = millis();
  
  // 检查是否超时（只有在曾经收到过数据的情况下才检查）
  if (lastPacketTime > 0 && currentTime - lastPacketTime > CONNECTION_TIMEOUT) {
    Serial.println("控制信号超时，执行紧急停止");
    emergencyStopMotors();
  }
}

// ===================== 驱动方案初始化 =====================

bool initializeDriver() {
  // 双MOS管方案 - 2个PWM
  ledcSetup(LEDC_CHANNEL_PWM1, LEDC_FREQUENCY, LEDC_TIMER_BITS);
  ledcAttachPin(PIN_PWM1, LEDC_CHANNEL_PWM1);

  ledcSetup(LEDC_CHANNEL_PWM2, LEDC_FREQUENCY, LEDC_TIMER_BITS);
  ledcAttachPin(PIN_PWM2, LEDC_CHANNEL_PWM2);

  writeSpeedToChannel(LEDC_CHANNEL_PWM1, 0);
  writeSpeedToChannel(LEDC_CHANNEL_PWM2, 0);

  dualmos_inited = true;
  Serial.println("双MOS管驱动初始化完成");
  return true;
}

// ===================== 电机控制 =====================

void controlMotorDualMos(int direction, int speed) {
  if (!dualmos_inited) {
    Serial.println("错误：双MOS管驱动未正确初始化");
    return;
  }

  // 重置紧急停止标志
  emergencyStop = false;
  
  // 先清零两路 PWM，避免同时导通
  writeSpeedToChannel(LEDC_CHANNEL_PWM1, 0);
  writeSpeedToChannel(LEDC_CHANNEL_PWM2, 0);

  if (direction == 1) {
    writeSpeedToChannel(LEDC_CHANNEL_PWM1, speed);
    Serial.printf("电机正转，速度: %d\n", speed);
  } else if (direction == -1) {
    writeSpeedToChannel(LEDC_CHANNEL_PWM2, speed);
    Serial.printf("电机反转，速度: %d\n", speed);
  } else {
    Serial.println("电机停止");
  }
}

// ===================== WiFi/AP/UDP =====================

bool setupAccessPoint() {
  Serial.println("正在启动WiFi热点...");

  // 启动 AP
  bool ok = WiFi.softAP(AP_SSID, AP_PASSWORD);
  if (!ok) {
    Serial.println("WiFi热点启动失败");
    return false;
  }

  IPAddress ip = WiFi.softAPIP();
  Serial.println("WiFi热点已启动");
  Serial.printf("SSID: %s\n", AP_SSID);
  Serial.printf("密码: %s\n", AP_PASSWORD);
  Serial.printf("IP地址: %s\n", ip.toString().c_str());

  // 启动 UDP 监听
  if (Udp.begin(UDP_PORT) == 1) {
    Serial.printf("开始监听UDP端口 %u...\n", UDP_PORT);
    return true;
  } else {
    Serial.println("UDP 启动失败");
    return false;
  }
}

// 解析控制数据：格式 "方向,速度"，方向 ∈ {-1,0,1}，速度 ∈ [0,255]
bool parseControlData(const String& data, int& direction, int& speed) {
  int commaIndex = data.indexOf(',');
  if (commaIndex < 0) {
    Serial.printf("数据格式错误: %s\n", data.c_str());
    direction = 0;
    speed = 0;
    return false;
  }

  String dirStr = data.substring(0, commaIndex);
  String spdStr = data.substring(commaIndex + 1);

  // 去除空白
  dirStr.trim();
  spdStr.trim();

  direction = dirStr.toInt();
  speed = spdStr.toInt();

  if (direction < -1 || direction > 1) {
    Serial.printf("方向值无效: %d\n", direction);
    direction = 0;
    speed = 0;
    return false;
  }
  if (speed < 0 || speed > 255) {
    Serial.printf("速度值无效: %d\n", speed);
    direction = 0;
    speed = 0;
    return false;
  }
  return true;
}

// 接收并处理 UDP 数据
void receiveControlDataOnce() {
  int packetSize = Udp.parsePacket();
  if (packetSize <= 0) {
    return; // 没有数据
  }

  // 读取数据
  char buffer[128];
  int len = Udp.read(buffer, sizeof(buffer) - 1);
  if (len <= 0) {
    return;
  }
  buffer[len] = '\0';

  String payload = String(buffer);
  payload.trim();

  int direction = 0;
  int speed = 0;
  bool ok = parseControlData(payload, direction, speed);

  // 更新最后收到数据包的时间
  lastPacketTime = millis();
  
  // 控制电机（即使解析失败也按 0,0 处理，等价于停止）
  controlMotorDualMos(direction, speed);

  // 打印来源与内容
  IPAddress remoteIp = Udp.remoteIP();
  uint16_t remotePort = Udp.remotePort();
  Serial.printf("收到来自 %s:%u 的数据: %s\n", remoteIp.toString().c_str(), remotePort, payload.c_str());

  if (!ok) {
    // 若数据非法，已在上方规范化为停止
  }
}

// ===================== Arduino 生命周期 =====================

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  delay(200);

  Serial.println("ESP32电机控制器启动...");

  // 初始化驱动
  if (!initializeDriver()) {
    Serial.println("驱动初始化失败，程序停止");
    return;
  }

  // 设置 WiFi AP + UDP
  if (!setupAccessPoint()) {
    Serial.println("WiFi热点设置失败，程序停止");
    return;
  }

  // 初始化电机控制，确保停止
  Serial.println("初始化电机控制...");
  stopAllMotors();
}

void loop() {
  // 轮询接收 UDP 控制数据
  receiveControlDataOnce();
  
  // 定期检查连接状态
  unsigned long currentTime = millis();
  if (currentTime - lastSafetyCheck >= SAFETY_CHECK_INTERVAL) {
    checkConnection();
    lastSafetyCheck = currentTime;
  }

  // 为了避免占用过高 CPU，可适当小延时
  delay(5);
}